#include <Servo.h>
#include <Arduino.h>
#define Servo_IO 16
#define PWMA 14
#define AIN1 13
#define AIN2 12
#define SYB 15

Servo myservo;
int midservo = 110;

int S_speed = 88;

void Turn_Init() {
  myservo.attach(Servo_IO);  //转向舵机
  myservo.write(90);
}

void Motor_Init() {
  //  pinMode(A0, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(SYB, OUTPUT);

  digitalWrite(AIN1, 0);
  digitalWrite(AIN2, 0);
  digitalWrite(SYB, 0);
  analogWrite(PWMA, S_speed);
}

//1为左，0为右
void turn_to(bool dir) {
  if (dir) {
    myservo.write(midservo+55);//左
  } else {
    myservo.write(midservo-35);
  }
}

void servo_mid() {
  myservo.write(midservo);
}

//1为转，0为反转
void GO_BACK(bool OR) {
  digitalWrite(SYB, 1);
  if (!OR) {
    digitalWrite(AIN1, 0);
    digitalWrite(AIN2, 1);
  } else {
    digitalWrite(AIN1, 1);
    digitalWrite(AIN2, 0);
  }
}

void SPEED(bool OR) {
  if (OR) {
    S_speed += 40;
    if (S_speed >= 255) S_speed = 255;
  } else {
    S_speed -= 40;
    if (S_speed <= 0) S_speed = 0;
  }
  analogWrite(PWMA, S_speed);
}
